
#include <Servo.h>
#include "ROB_Telemetry.h"
#include "ROB_Pantilt.h"




void CPantilt::Begin(uint8_t panPin, uint8_t tiltPin) {

  // Attach servos
  _servoPan.attach(panPin); 
  _servoTilt.attach(tiltPin);

  // Move to mid pos
  _servoPan.write(SERVO_PAN_MID_POS);
  _servoTilt.write(SERVO_TILT_MID_POS);
}



void CPantilt::update(void) {

  if(tmy.servoTiltPos < tmy.servoTiltTargetPos &&
    tmy.servoTiltTargetPos <= SERVO_TILT_MAX_POS) {
    _servoTilt.write(++tmy.servoTiltPos);
  }
  else if (tmy.servoTiltPos > tmy.servoTiltTargetPos &&
    tmy.servoTiltTargetPos >= SERVO_TILT_MIN_POS) {
    _servoTilt.write(--tmy.servoTiltPos);
  }
  if(tmy.servoPanPos < tmy.servoPanTargetPos && 
    tmy.servoPanTargetPos <= SERVO_PAN_MAX_POS) {
    _servoPan.write(++tmy.servoPanPos);
  }
  else if (tmy.servoPanPos > tmy.servoPanTargetPos &&
    tmy.servoPanTargetPos >= SERVO_PAN_MIN_POS) {
    _servoPan.write(--tmy.servoPanPos);
  }
}


CPantilt Pantilt;

